#include <iostream>
#include <list>
#include <pthread.h>
#include <unistd.h>

#include "vectornav/vectornav.h"

using namespace std;

/* VN100 */
Vn100		vn100;
VnYpr		attitude;
VnVector3	magnetic;
VnVector3	acceleration;
VnVector3	angularRate;

/* Threads */
pthread_t	readerThread;
pthread_t	counterThread;
pthread_t	processorThread;

void		*reader		(void *arg);
void		*counter	(void *arg);
void		*processor	(void *arg);

/* Samples */
struct		sample {
	VnVector3	acceleration;
	VnVector3	angularRate;
};

list<sample>	samples;

/* Counter */
int			sampleCounter;
int			counterIterationCounter;

int main()
{
	// Set the connection variables
	const char	*port		=	"//dev//ttyUSB0";
	const int	baudRate	=	230400;

	// Connect
	vn100_connect(&vn100, port, baudRate);

	// Prepare
	sampleCounter				=	0;
	counterIterationCounter		=	0;

	// Create threads
	cout << "Application started" << endl;
	pthread_create(&readerThread,		NULL,	reader,		(void *)NULL);
	pthread_create(&counterThread,		NULL,	counter,	(void *)NULL);
	pthread_create(&processorThread,	NULL,	processor,	(void *)NULL);

	// Main loop (keeps the app running)
	while(true) {

	}
	return 0;
}

/**
 * Reads data from the VN100.
 */
void *reader(void *arg)
{
	cout << "Reader started" << endl;

	while(true) {
		// Read VN100 data
		vn100_getYawPitchRollMagneticAccelerationAngularRate(
				&vn100,
				&attitude,
				&magnetic,
				&acceleration,
				&angularRate);

		// Create a sample
		sample currentSample;
		currentSample.acceleration = acceleration;
		currentSample.angularRate = angularRate;

		// Append the sample to the list
		samples.push_back(currentSample);

		// Count
		sampleCounter++;
	}
	return (void *)NULL;
}

/**
 * Counts the amount of samples per second.
 */
void *counter(void *arg)
{
	cout << "Counter started" << endl;

	while(true) {
		// Sleep for a second
		usleep(1000000);

		// Increase the iteration counter
		counterIterationCounter++;

		// Print the amount of samples
		cout << counterIterationCounter << ":\t\t" << sampleCounter << "\t\t" << samples.size() << endl;
		sampleCounter = 0;
	}
	return (void *)NULL;
}

/**
 * Processes the samples.
 */
void *processor(void *arg)
{
	cout << "Sample processor started" << endl << endl;

	while(true) {
		if (samples.size() > 0) {
			// Apply filter
		}
	}
	return (void *)NULL;
}


































